/*
 * Homography.cpp
 *
 *  Created on: Apr 24, 2012
 *      Author: fdesorbi
 *     Contact: fdesorbi@hvrl.ics.keio.ac.jp
 */

#include "hvrl/tools/Homography.hpp"

namespace hvrl
{

namespace Homography
{

void jacobianLines(const cv::Size& size, cv::Mat& output)
{
	// Create the jacobian matrix
	output = cv::Mat(16, size.height * size.width, cv::DataType<float>::type,
			0.0f);

	// derivate over x from lines 0 to 8
	// derivate over y from lines 9 to 15
	for (int i = 0; i < size.height; i++)
	{
		for (int j = 0; j < size.width; j++)
		{
			output.at<float> (0, (i * size.width) + j) = output.at<float> (11,
					(i * size.width) + j) = j;

			output.at<float> (1, (i * size.width) + j) = output.at<float> (12,
					(i * size.width) + j) = i;

			output.at<float> (2, (i * size.width) + j) = output.at<float> (13,
					(i * size.width) + j) = 1;

			output.at<float> (7, (i * size.width) + j) = output.at<float> (14,
					(i * size.width) + j) = -i * j;

			output.at<float> (6, (i * size.width) + j) = -j * j;

			output.at<float> (15, (i * size.width) + j) = -i * i;
		}
	}

}

void jacobianBlocks(const cv::Size& size, cv::Mat& output)
{
	// Create the jacobian matrix
	output = cv::Mat(2 * size.height, 8 * size.width,
			cv::DataType<float>::type, 0.0f);

	for (int i = 0; i < size.height; i++)
	{
		for (int j = 0; j < size.width; j++)
		{
			output.at<float> (i, j) = output.at<float> (size.height + i,
					3 * size.width + j) = j;

			output.at<float> (i, size.width + j) = output.at<float> (
					size.height + i, 4 * size.width + j) = i;

			output.at<float> (i, 2 * size.width + j) = output.at<float> (
					size.height + i, 5 * size.width + j) = 1;

			output.at<float> (i, 7 * size.width + j) = output.at<float> (
					size.height + i, 6 * size.width + j) = -i * j;

			output.at<float> (i, 6 * size.width + j) = -j * j;

			output.at<float> (size.height + i, 7 * size.width + j) = -i * i;
		}
	}

}

void steepestDescentImageLines(const cv::Mat& gradientX,
		const cv::Mat& gradientY, const cv::Mat& jacobian, cv::Mat& output)
{
	// gradientX anf gradientY might have the same dimensions
	output = cv::Mat(8, gradientX.rows * gradientX.cols, jacobian.type());
	// For each derivate
	for (unsigned int derivate = 0; derivate < 8; ++derivate)
	{
		for (int i = 0; i < gradientX.rows; ++i)
		{
			for (int j = 0; j < gradientX.cols; ++j)
			{
				int k = i * gradientX.cols + j;
				output.at<float> (derivate, k) = gradientX.at<float> (i, j)
						* jacobian.at<float> (derivate, k)
						+ gradientY.at<float> (i, j) * jacobian.at<float> (
								8 + derivate, k);
			}
		}

	}
}

void steepestDescentImageBlocks(const cv::Mat& gradientX,
		const cv::Mat& gradientY, const cv::Mat& jacobian, cv::Mat& output)
{
	// gradientX anf gradientY might have the same dimensions
	output = cv::Mat(gradientX.rows, 8 * gradientX.cols, jacobian.type());

	// For each derivate
	for (unsigned int derivate = 0; derivate < 8; ++derivate)
	{
		for (int i = 0; i < gradientX.rows; ++i)
		{
			for (int j = 0; j < gradientX.cols; ++j)
			{
				output.at<float> (i, derivate * gradientX.cols + i)
						= gradientX.at<float> (i, j) * jacobian.at<float> (i,
								derivate * gradientX.cols + j) + gradientY.at<
								float> (i, j) * jacobian.at<float> (
								gradientX.rows + i,
								derivate * gradientX.cols + j);
			}
		}

	}
}



} /* Homography */

} /* hvrl */
